/*
  1、不同目标点之间进行循环移动
  2、到达每个目标点时播放对应的语音提示（需获取到tf）
  3、到达目标点通过摄像头拍摄实时图像并保存到本地
 */
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "nav2_msgs/action/navigate_to_pose.hpp"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "sensor_msgs/msg/image.hpp"
#include "opencv4/opencv2/opencv.hpp"
#include "cv_bridge/cv_bridge.h"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_msgs/srv/set_initial_pose.hpp"
#include "autopartol_interface/srv/speech_text.hpp"

using nav2_msgs::action::NavigateToPose;
using std::placeholders::_1;
using std::placeholders::_2;
using namespace std::chrono_literals;
using autopartol_interface::srv::SpeechText;
using nav2_msgs::srv::SetInitialPose;
using sensor_msgs::msg::Image;

class PartolRobot : public rclcpp::Node
{
private:
  std::shared_ptr<tf2_ros::Buffer> buffer_;
  rclcpp_action::Client<NavigateToPose>::SharedPtr pose_client_;
  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
  rclcpp::Subscription<Image>::SharedPtr image_;
  std::string save_path_;
  int photo_count_;
  rclcpp::Client<SetInitialPose>::SharedPtr initial_pose_client_;
  rclcpp::Client<SpeechText>::SharedPtr speech_client_;

  void initial_pose()
  {
    while (!initial_pose_client_->wait_for_service(5s))
    {
      RCLCPP_INFO(this->get_logger(), "等待服务器set_initial_pose响应！");
    }

    auto request = std::make_shared<SetInitialPose::Request>();
    request->pose.header.frame_id = "map";
    request->pose.header.stamp = this->get_clock()->now();
    request->pose.pose.pose.position.x = 0.0;
    request->pose.pose.pose.position.y = 0.0;
    request->pose.pose.pose.orientation.w = 1.0;

    auto future = initial_pose_client_->async_send_request(request);

    try
    {
      auto response = rclcpp::spin_until_future_complete(this->get_node_base_interface(), future);
      if (response == rclcpp::FutureReturnCode::SUCCESS)
      {
        RCLCPP_INFO(this->get_logger(), "初始化位姿成功!");
      }
      else if (response == rclcpp::FutureReturnCode::INTERRUPTED)
      {
        RCLCPP_INFO(this->get_logger(), "被打断！");
      }
      else
      {
        RCLCPP_INFO(this->get_logger(), "超时！");
      }
    }
    catch (const std::exception &e)
    {
      RCLCPP_ERROR(this->get_logger(), "呼叫服务器失败:%s", e.what());
    }
  }

  void image_callback(const sensor_msgs::msg::Image::SharedPtr msg)
  {
    try
    {
      // 格式转换：msg->opencv
      cv::Mat image = cv_bridge::toCvCopy(msg, "bgr8")->image;
      std::string filename = save_path_ + "image_" + std::to_string(this->get_clock()->now().seconds()) + ".jpg";
      cv::imwrite(filename, image);
      RCLCPP_INFO(this->get_logger(), "图片已保存到:%s", filename.c_str());
    }
    catch (const cv_bridge::Exception &e)
    {
      RCLCPP_ERROR(this->get_logger(), "图片转换失败:%s", e.what());
    }
  }

public:
  PartolRobot() : Node("partol_robot_node"), save_path_("/home/ubuntu/chapt7/chapt7_ws/src/autopartol_robot/image/"), photo_count_(0)
  {
    buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock(), tf2::Duration(5s));
    pose_client_ = rclcpp_action::create_client<NavigateToPose>(this, "/navigate_to_pose");
    tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*buffer_);
    initial_pose_client_ = this->create_client<SetInitialPose>("/set_initial_pose");
    speech_client_ = this->create_client<SpeechText>("speech_text");
    image_ = this->create_subscription<Image>("/camera_sensor/image_raw", 10, std::bind(&PartolRobot::image_callback, this, _1));
    initial_pose();
  }

  std::tuple<double, double, double> get_tf()
  {
    try
    {
      auto listener = buffer_->lookupTransform("map", "base_footprint", tf2::TimePointZero, 5s);
      double x = listener.transform.translation.x;
      double y = listener.transform.translation.y;
      double w = listener.transform.rotation.w;
      RCLCPP_INFO(this->get_logger(),
                  "父坐标系:%s,子坐标系:%s,偏移量(x:%.2f,y:%.2f,z:%.2f)",
                  listener.header.frame_id.c_str(),
                  listener.child_frame_id.c_str(),
                  x, y, w);
      return std::make_tuple(x, y, w);
    }
    catch (const tf2::TransformException &ex)
    {
      RCLCPP_ERROR(this->get_logger(), "TF异常: %s", ex.what());
      return std::make_tuple(0.0, 0.0, 0.0);
    }
  }

  void speech()
  {
    while (!speech_client_->wait_for_service(5s))
    {
      RCLCPP_INFO(this->get_logger(), "等待服务器speech_text响应！");
    }
    auto req = std::make_shared<SpeechText::Request>();
    auto [x, y, w] = get_tf();
    req->text = "坐标 (x = " + std::to_string(x) + " , y = " + std::to_string(y) + " , w = " + std::to_string(w) + ")";

    auto future = speech_client_->async_send_request(req);
    try
    {
      auto responce = rclcpp::spin_until_future_complete(this->get_node_base_interface(), future);
      if (responce == rclcpp::FutureReturnCode::SUCCESS)
      {
        RCLCPP_INFO(this->get_logger(), "语音播报成功!");
      }
      else if (responce == rclcpp::FutureReturnCode::INTERRUPTED)
      {
        RCLCPP_INFO(this->get_logger(), "语音播报被打断!");
      }
      else
      {
        RCLCPP_INFO(this->get_logger(), "语音播报超时!");
      }
    }
    catch (const std::exception &e)
    {
      RCLCPP_ERROR(this->get_logger(), "呼叫服务器失败:%s", e.what());
    }
  }

  void send_goal_pose()
  {
    // 位置数组
    std::vector<std::tuple<float, float, float>> goal_positions_ = {
        {1.0, 1.0, 1.0},
        {2.0, 0.0, 1.0},
        {0.5, 1.0, 1.0}};

    if (!pose_client_->wait_for_action_server())
    {
      RCLCPP_INFO(this->get_logger(), "连接/navigation_to_pose动作服务器失败！");
    }
    for (const auto &[x, y, orientation] : goal_positions_)
    {
      auto goal_pose = NavigateToPose::Goal();
      auto stamp = this->get_clock()->now();

      goal_pose.pose.header.stamp = stamp;
      goal_pose.pose.header.frame_id = "map";
      goal_pose.pose.pose.position.x = x;
      goal_pose.pose.pose.position.y = y;
      goal_pose.pose.pose.orientation.w = orientation;

      rclcpp_action::Client<NavigateToPose>::SendGoalOptions options;
      options.feedback_callback = std::bind(&PartolRobot::Feedback_Callback, this, _1, _2);
      options.goal_response_callback = std::bind(&PartolRobot::GoalResponseCallback, this, _1);
      options.result_callback = std::bind(&PartolRobot::ResultCallback, this, _1);

      pose_client_->async_send_goal(goal_pose, options);
      rclcpp::sleep_for(5s);
    }
  }

  void Feedback_Callback(rclcpp_action::ClientGoalHandle<NavigateToPose>::SharedPtr goalhandle, const std::shared_ptr<const NavigateToPose::Feedback> feedback)
  {
    (void)goalhandle;
    RCLCPP_INFO(this->get_logger(),
                "导航运行时间：%d,x轴坐标:%.2f,y轴坐标:%.2f,转向角:%.2f,剩余距离:%.2f",
                feedback->navigation_time.sec,
                feedback->current_pose.pose.position.x,
                feedback->current_pose.pose.position.y,
                feedback->current_pose.pose.orientation.w,
                feedback->distance_remaining);
  }

  void GoalResponseCallback(rclcpp_action::ClientGoalHandle<NavigateToPose>::SharedPtr goalhandle)
  {
    if (!goalhandle)
    {
      RCLCPP_INFO(this->get_logger(), "目标位置被/navigation_to_pose动作服务端拒绝！");
    }
    else
    {
      RCLCPP_INFO(this->get_logger(), "目标位置被/navigation_to_pose动作服务端接收！");
    }
  }

  void ResultCallback(const rclcpp_action::ClientGoalHandle<NavigateToPose>::WrappedResult &result)
  {
    switch (result.code)
    {
    case rclcpp_action::ResultCode::SUCCEEDED:
      RCLCPP_INFO(this->get_logger(), "导航到目标点成功!");
      PartolRobot::get_tf();
      PartolRobot::speech();
      if (photo_count_ < 3)
      {
        auto fake_image = std::make_shared<Image>();
        PartolRobot::image_callback(fake_image);
        photo_count_++;
      }

      break;
    case rclcpp_action::ResultCode::CANCELED:
      RCLCPP_INFO(this->get_logger(), "导航到目标点任务被取消!");
      break;
    case rclcpp_action::ResultCode::ABORTED:
      RCLCPP_INFO(this->get_logger(), "导航到目标点中断！");
      break;
    default:
      RCLCPP_INFO(this->get_logger(), "未知异常！");
      break;
    }
  }
};

int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<PartolRobot>();
  node->send_goal_pose();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}
